import numpy as np
import gym

class AdaptivePID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.integral = 0.0
        self.last_error = 0.0

    def update(self, setpoint, feedback):
        error = setpoint - feedback
        self.integral += error
        derivative = error - self.last_error
        # 自适应调整PID参数
        self.Kp += self.Kp * 0.01 * error
        self.Ki += self.Ki * 0.01 * self.integral
        self.Kd += self.Kd * 0.01 * derivative
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output

env = gym.make('CartPole-v1', render_mode="human")
desired_state = np.array([0, 0, 0, 0])
pid = AdaptivePID(Kp=0.1, Ki=0.01, Kd=0.5)

N_episodes = 10
N_steps = 100000

for i_episode in range(N_episodes):
    state, _ = env.reset()
    for t in range(N_steps):
        env.render()
        error = state - desired_state
        control = pid.update(desired_state, state)
        print(control)
        action = 0 if control > 0 else 1
        state, reward, done, _, _ = env.step(action)
        if done:
            print(f"Episode finished after {t} timesteps")
            break
env.close()